/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CGridGServerModule.h"

/**
* Constructor
*/
CGridGServerModule::CGridGServerModule(gimi::GIMI *g){
	isHeader = false;
	index = 0;
	maxUnsync = 1;
	timeout = 1000;
	
	if(g){
		if(g->isConnected()){
			gim=g;
		}else{
			fprintf(stderr,"ImageClient ERROR: You should first initialize the GIMI and then ImageClient\n");
			exit(1);
		}
	}else{
			fprintf(stderr,"ImageClient ERROR: GIMI seems to be NULL, initialize it first\n");
			exit(1);
		}
		gim->addProvidedService(GIMI_PROTOCOL_GRID,0,"Grid data");
		
}
CGridGServerModule::~CGridGServerModule(){
  
}
void CGridGServerModule::setHeader(TGIMI_PROTOCOL_GRID_HEADER head){
    header = head;
		isHeader = true;
}

/**
* Set the header by components
* Must be called once before send!!
*/
void CGridGServerModule::setHeader(int32_t g_size, int32_t g_Nx, int32_t g_Ny, float g_resolution, 
								unsigned char g_format, int32_t or_x, int32_t or_y)
{
	header.size = g_size;
	header.Nx = g_Nx;
	header.Ny = g_Ny;
	header.resolution = g_resolution;
	header.format = g_format;
	header.orig_x = or_x;
	header.orig_y = or_y;
	isHeader = true;
}

/**
	* Send grid data. The header must be set before
	* The grid is given as unsigned char buffer and the header size should be bytes 
	* 
	*/
bool CGridGServerModule::sendGrid(unsigned char *grid){
	if(!isHeader) return false;
	netPacket.setGrid(grid, header);
	int retval = gim->sendToSubscribers((const char *) netPacket.getNetPacket(),
													netPacket.size(),
													GIMI_PROTOCOL_GRID,
													0,
													index++,
													0, 
													maxUnsync,
													timeout);
	
	return (retval==GIMI_OK);
}
bool CGridGServerModule::sendGrid(unsigned char *grid, int32_t t_sec,int32_t t_usec){
	header.t_sec = t_sec;
	header.t_usec = t_usec;
	return sendGrid(grid);
}
bool CGridGServerModule::sendGrid(unsigned char *grid, float xx,float yy,float aa){
	header.x = xx;
	header.y = yy;
	header.a = aa;
	return sendGrid(grid);
}

/**
	* Set time to header
	*/
void CGridGServerModule::setTime(int32_t t_sec,int32_t t_usec){
	header.t_sec = t_sec;
	header.t_usec = t_usec;
}
/**
	* Set the pose to header
	*/
void CGridGServerModule::setPose(float xx,float yy,float aa){
	header.x = xx;
	header.y = yy;
	header.a = aa;
}

void CGridGServerModule::setSendParameters(int32_t sync_timeout, int32_t maxUnsynced){
  timeout = sync_timeout;
	maxUnsync = maxUnsynced;
}



